#include "DataLoder.h"
#include <algorithm>


DataLoder::DataLoder(){}

DataLoder::DataLoder(Paras pars):pars_(pars)
{
    AllData_.resize(pars.max_picture_num);
    int temp = pars.n_strat_+pars.max_picture_num;
#pragma omp parallel for
    for(int i=pars.n_strat_;i<temp;i++)
    {
        std::string c_path_sub = str_format("/data/img/%05d.jpg",i);
        std::string d_path_sub = str_format("/data/depth_filtered/%05d.jpg",i);
        std::string c_path = pars_.datapath+c_path_sub;
        std::string d_path = pars_.datapath+d_path_sub;

        DataTuple data;
        data.c_img = cv::imread(c_path);
        data.d_img = cv::imread(d_path,CV_16U);
        // AllData_.push_back(data);

        AllData_[i-pars_.n_strat_]=data;
        
    }

}

DataLoder::~DataLoder()
{
}

DataTuple DataLoder::Getframes(int t){
    return AllData_[t];
}

PointCloud DataLoder::PcRecover(int t){
    PointCloud pc;
    DataTuple framet = Getframes(t);
    for(int u = 0; u < framet.c_img.rows; u++)
    {
        for(int v = 0; v < framet.c_img.cols; v++)
        {
            auto z = framet.d_img.at<uchar>(u,v);
            if(z>=10)
            {
                Eigen::Vector3d pixel_coor(v,u,1);
                Eigen::Vector3d color;
                cv::Vec3b clt = framet.c_img.ptr<cv::Vec3b>(u)[v];
                auto point = z*pars_.K.inverse()*pixel_coor;
                
                color << clt[2]/255.0,clt[1]/255.0,clt[0]/255.0;
                pc.points_.push_back(point);
                pc.colors_.push_back(color);
                
            }


        }
    }
    return pc;
}
